template <class T>
Quaternion<T>::Quaternion(void)
:r(1),x(0),y(0),z(0)
{
}

template <class T>
Quaternion<T>::Quaternion(T _r,T _x,T _y,T _z)
:r(_r),x(_x),y(_y),z(_z)
{
}

template <class T>
Quaternion<T>::Quaternion(const Vector4<T>& vec)
{
	r = vec.w;
	x = vec.x;
	y = vec.y;
	z = vec.z;
}

template <class T>
Quaternion<T>::Quaternion(const Vector3<T>& axis, T radius)
{
	// assert:  axis[] is unit length
	//
	// The quaternion representing the rotation is
	// q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

	T fHalfRadius = radius * 0.5f;
	T fSin = sin(fHalfRadius);
	r = cos(fHalfRadius);
	x = fSin * axis.x;
	y = fSin * axis.y;
	z = fSin * axis.z;
}

template <class T>
T Quaternion<T>::dot(const Quaternion<T>& rhs) const
{
	T result = 0;
	result = r*rhs.r+x*rhs.x+y*rhs.y+z*rhs.z;
	return result;
}

template <class T>
Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& rhs) const
{
	Quaternion<T> result;
	result.r = r * rhs.r - x * rhs.x - y * rhs.y - z * rhs.z;
	result.x = r * rhs.x + x * rhs.r + y * rhs.z - z * rhs.y;
	result.y = r * rhs.y + y * rhs.r - x * rhs.z + z * rhs.x;
	result.z = r * rhs.z + z * rhs.r + x * rhs.y - y * rhs.x;
	return result;
}

template <class T>
Vector3<T> Quaternion<T>::operator*(const Vector3<T>& rhs) const
{
	// nVidia SDK implementation
	Vector3<T> uv, uuv;
	Vector3<T> qvec(x, y, z);
	uv = qvec.cross(rhs);
	uuv = qvec.cross(uv);
	uv *= (2.0f * r);
	uuv *= 2.0f;

	return rhs + uv + uuv;
}

template <class T>
Quaternion<T> Quaternion<T>::operator+(const Quaternion<T>& rhs) const
{
	Quaternion<T> result;
	result.r = r + rhs.r;
	result.x = x + rhs.x;
	result.y = y + rhs.y;
	result.z = z + rhs.z;
	return result;
}

template <class T>
Quaternion<T> Quaternion<T>::operator-(const Quaternion<T>& rhs) const
{
	Quaternion<T> result;
	result.r = r - rhs.r;
	result.x = x - rhs.x;
	result.y = y - rhs.y;
	result.z = z - rhs.z;
	return result;
}

template <class T>
Quaternion<T> Quaternion<T>::operator*(T scalar) const
{
	Quaternion<T> result;
	result.r = r * scalar;
	result.x = x * scalar;
	result.y = y * scalar;
	result.z = z * scalar;
	return result;
}

template <class T>
Quaternion<T> Quaternion<T>::operator/(T scalar) const
{
	Quaternion<T> result;
	result.r = r / scalar;
	result.x = x / scalar;
	result.y = y / scalar;
	result.z = z / scalar;
	return result;
}

template <class T>
Quaternion<T>& Quaternion<T>::operator*=(T scalar)
{
	r *= scalar;
	x *= scalar;
	y *= scalar;
	z *= scalar;
	return (*this);
}

template <class T>
Quaternion<T> Quaternion<T>::operator-() const
{
	Quaternion<T> q;
	q.r = -r;
	q.x = -x;
	q.y = -y;
	q.z = -z;
	return (q);
}

template <class T>
Quaternion<T> Quaternion<T>::conjugate() const
{
	Quaternion<T> q;
	q.r = r;
	q.x = -x;
	q.y = -y;
	q.z = -z;
	return q;
}

template <class T>
Quaternion<T>& Quaternion<T>::normalize()
{
	T t = sqrt(r * r + x * x + y * y + z * z);
	r /= t;
	x /= t;
	y /= t;
	z /= t;
	return (*this);
}

template <class T>
Quaternion<T> Quaternion<T>::slerp(const Quaternion<T>& q1, const Quaternion<T>& q2, T t)
{
	if (t <= (T)0.0)
		return q1;
	if (t >= (T)1.0)
		return q2;

	Quaternion<T> q3 = q2;
	T c = q1.dot(q3);

	if (c < (T)0.0)
	{
		q3 = -q3;
		c = -c;
	}

	if (c > (T)1.0 - _epsilon)
		return (q1 + (q2 - q1) * t).Normalize();

	T a = acos(c); // ACos() clamps input to [-1, 1]
	return (q1 * sin(((T)1.0 - t) * a) + q3 * sin(t * a)) / sin(a);
}

template <class T>
void Quaternion<T>::getMatrix(Matrix44<T>& mat4) const
{
	// todo assert unit quaternion
	T fx  = 2.0f * x;
	T fy  = 2.0f * y;
	T fz  = 2.0f * z;
	T frx = fx * r;
	T fry = fy * r;
	T frz = fz * r;
	T fxx = fx * x;
	T fxy = fy * x;
	T fxz = fz * x;
	T fyy = fy * y;
	T fyz = fz * y;
	T fzz = fz * z;

	// row major
	mat4[0] = 1.0f - (fyy + fzz);
	mat4[1] = fxy - frz;
	mat4[2] = fxz + fry;
	mat4[3] = 0;

	mat4[4] = fxy + frz;
	mat4[5] = 1.0f - (fxx + fzz);
	mat4[6] = fyz - frx;
	mat4[7] = 0;

	mat4[8] = fxz - fry;
	mat4[9] = fyz + frx;
	mat4[10] = 1.0f - (fxx + fyy);
	mat4[11] = 0;

	mat4[12] = 0;
	mat4[13] = 0;
	mat4[14] = 0;
	mat4[15] = 1;
}